// --------------------------------------------------------------------------------------------------------------------
// <copyright file="LandmarksInfoDataLoaderTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{
    using System;
    using System.Diagnostics;
    using System.IO;
    using System.Windows.Media.Imaging;

    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Microsoft.VisualStudio.TestTools.UnitTesting;    
    
    /// <summary>
    /// Landmarks info data loader related tests.
    /// </summary>
    [TestClass]
    public class LandmarksInfoDataLoaderTest
    {
        /// <summary>
        /// Test Landmark Points
        /// </summary>
        private static readonly Landmark[] LandmarkPoints = 
         {
            new Landmark(
                0,
                new Pose2D(0, 0, 0),
                new LandmarkTiltParams(0.99942 * MathConstants.Degrees2Radians, -0.92663  * MathConstants.Degrees2Radians)),
            new Landmark(
                1,
                new Pose2D(16.702, 0,  0.012319),
                new LandmarkTiltParams(0.75641 * MathConstants.Degrees2Radians, -0.73852 * MathConstants.Degrees2Radians)),
            new Landmark(
                2,
                new Pose2D(34.981, 5.2089, 0.01013),
                new LandmarkTiltParams(1.3281 * MathConstants.Degrees2Radians, -0.54959 * MathConstants.Degrees2Radians)),
            new Landmark(
                3,
                new Pose2D(51.236, 5.3524, -0.0011319),
                new LandmarkTiltParams(1.4351 * MathConstants.Degrees2Radians, -0.071814 * MathConstants.Degrees2Radians)),
            new Landmark(
                4,
                new Pose2D(51.309, -6.9505, -0.062455),
                new LandmarkTiltParams(1.0615 * MathConstants.Degrees2Radians, -1.6034 * MathConstants.Degrees2Radians)),
            new Landmark(
                5,
                new Pose2D(17.141, -7.2355,  0.025545),
                new LandmarkTiltParams(1.1447 * MathConstants.Degrees2Radians, -0.53158 * MathConstants.Degrees2Radians)),
            new Landmark(
                6,
                new Pose2D(8.253, -13.509, -0.022689),
                new LandmarkTiltParams(0.47554 * MathConstants.Degrees2Radians, -1.8111 * MathConstants.Degrees2Radians)),
            new Landmark(
                7,
                new Pose2D(0.14126, -13.426, -0.00058451),
                new LandmarkTiltParams(-0.22434 * MathConstants.Degrees2Radians, -0.76435 * MathConstants.Degrees2Radians)),
        };

        /// <summary>
        /// Tests landmark data loader export/import.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void LandmarkDataSaveAndLoadTest()
        {
            string testFileName = LandmarksInfo.Filename;
            LandmarksInfo testLandmarkInfo = new LandmarksInfo(LandmarkPoints);

            DataLoader<LandmarksInfo>.ExportData(testFileName, testLandmarkInfo);
            LandmarksInfo readLandmarkInfo = DataLoader<LandmarksInfo>.ImportData(testFileName);  
            Assert.AreEqual(testLandmarkInfo.LandmarkPoints.Length, readLandmarkInfo.LandmarkPoints.Length);

            // Compare the loaded landmarks to the test landmarks.
            for (int i = 0; i < testLandmarkInfo.LandmarkPoints.Length; ++i)
            {
                Assert.AreEqual(testLandmarkInfo.LandmarkPoints[i].Id, readLandmarkInfo.LandmarkPoints[i].Id);
                Assert.IsTrue(testLandmarkInfo.LandmarkPoints[i].Pose == readLandmarkInfo.LandmarkPoints[i].Pose);
                var testTiltCalib = testLandmarkInfo.LandmarkPoints[i].TiltCalibrationParams;
                var readTiltCalib = readLandmarkInfo.LandmarkPoints[i].TiltCalibrationParams;

                Assert.AreEqual(testTiltCalib.FloorAzimuthAngle, readTiltCalib.FloorAzimuthAngle);
                Assert.AreEqual(testTiltCalib.FloorTiltAngle, readTiltCalib.FloorTiltAngle);              
            }    
        }
    }
}
